#pragma once
namespace test
{
   using namespace graph;
   struct dijkstra_sh_path_t
   {
      dijkstra_sh_path_t(graph_t const & graph, vertex_id_t start_v_id, vertex_id_t finish_v_id)
         :  graph_(graph)
         ,  start_v_id_(start_v_id)
         ,  finish_v_id_(finish_v_id)
         ,  completed_(true)
      {run();}
      
      std::list<lat_lon_t> sh_path_;
      bool completed_; //if shotest path exists between given vertices
   private:
      graph_t const & graph_;
      vertex_id_t start_v_id_;
      vertex_id_t finish_v_id_;
      struct vertex_inf_t
      {
         vertex_inf_t()
            :  color_(NOT_VISITED_COLOR)
            ,  pred_(0)
            ,  dist_(MAX_FLOAT)
         {}
         vertex_inf_t(char color, vertex_id_t pred, lat_lon_t dist)
            :  color_(color)
            ,  pred_(pred)
            ,  dist_(dist)
         {}
         char color_;
         vertex_id_t pred_;
         lat_lon_t dist_;
         static char const NOT_VISITED_COLOR = 0;
         static char const EXPLORED_COLOR = 1;
         static char const PROCESSED_COLOR = 2;
      };      
      boost::unordered_map<vertex_id_t, vertex_inf_t> inf_;
      std::list<vertex_id_t> min_dist_queue;
      void init()
      {
         min_dist_queue.push_back( start_v_id_); 
         inf_[start_v_id_] = vertex_inf_t();
         inf_[start_v_id_].dist_ = 0;
         vertex_t const start_v = graph_[start_v_id_]; 
         //std::vector<vertex_id_t> nearest = graph_.get_nearest_vertices(graph_[start_v_id_].get_point(), INITIAL_ALG_VERTEX_COUNT);
         /*for(std::vector<vertex_id_t>::iterator it = nearest.begin(); it != nearest.end(); ++it)
         {
            if ((*it) == start_v_id_)
               continue;
            min_dist_queue.push_back(*it);
            inf_[*it] = vertex_inf_t(vertex_inf_t::NOT_VISITED_COLOR, start_v_id_, math::distance(start_v.get_point(), graph_[*it].get_point()));
         }         */
      }
      void fill_sh_path()
      {
         vertex_id_t temp_id = finish_v_id_;
         sh_path_.clear();
         while(temp_id != start_v_id_)
         {
            vertex_t cur_v = graph_[temp_id];
            sh_path_.push_back(cur_v.get_point().lat_);
            sh_path_.push_back(cur_v.get_point().lon_);       
            temp_id = inf_[temp_id].pred_;
         }
         vertex_t cur_v = graph_[temp_id];
         sh_path_.push_back(cur_v.get_point().lat_);
         sh_path_.push_back(cur_v.get_point().lon_);   
      }
      vertex_id_t extract_min_fwd()
      {
         if (min_dist_queue.empty())
            return 0;
         std::list<vertex_id_t>::iterator it = min_dist_queue.begin();
         std::list<vertex_id_t>::iterator min_it = it;
         lat_lon_t min_dist = MAX_FLOAT;
         for(; it != min_dist_queue.end(); ++it)
         {
            lat_lon_t cur_dist = inf_[(*it)].dist_;
            if (min_dist > cur_dist)
            {
               min_it = it;
               min_dist = cur_dist;
            }
         }
         vertex_id_t min_id = (*min_it);
         min_dist_queue.erase(min_it);
         return min_id;
      }
      void run()
      {
         init();      
         vertex_id_t cur_v_id = start_v_id_;   
         while(cur_v_id != finish_v_id_ && !min_dist_queue.empty())
         {
            cur_v_id = extract_min_fwd();
            vertex_inf_t& cur_v_inf = inf_[cur_v_id];
            cur_v_inf.color_ = vertex_inf_t::PROCESSED_COLOR;
            adj_vertices_iterator_pair_t<graph::forward_adj>::type
               it_pair = adj_vertices<graph::forward_adj>(cur_v_id, const_cast<graph_t&>(graph_));
            for(; it_pair.first != it_pair.second; ++it_pair.first)
            {
               vertex_inf_t& adj_v_inf = inf_[(*it_pair.first).get_id()];
               if (adj_v_inf.color_ == vertex_inf_t::PROCESSED_COLOR)
                  continue;
               if (adj_v_inf.color_ == vertex_inf_t::NOT_VISITED_COLOR)
               {
                  min_dist_queue.push_back((*it_pair.first).get_id());
                  adj_v_inf.color_ = vertex_inf_t::EXPLORED_COLOR; 
               }

               lat_lon_t new_dist = cur_v_inf.dist_ + math::distance(graph_[cur_v_id].get_point(), (*it_pair.first).get_point());
               if (adj_v_inf.dist_ > new_dist)
               {
                  adj_v_inf.dist_ = new_dist;
                  adj_v_inf.pred_ = cur_v_id;
               }
            }            
         }         
         completed_ = cur_v_id == finish_v_id_;
         if (completed_)
            fill_sh_path();
      }
   };
}